#pragma once
#include "GePoint3.h"
namespace tge{

	template<typename RealT, typename PointType, typename VectorType>
	class TGePlane
	{
	public:
		typedef RealT value_type;
		RealT A, B, C, D;
	public:
		TGePlane()
		{
			A = 0;
			B = 0;
			C = 0;
			D = 0;
		}
		TGePlane(RealT a, RealT b, RealT c, RealT d)
			:A(a), B(b), C(c), D(d)
		{}
		TGePlane(const PointType &p0, const VectorType &normal)
		{
			set(p0, normal);
		}
		TGePlane& set(const PointType&p0, const VectorType&normal)
		{
			A = normal.x;
			B = normal.y;
			C = normal.z;
			D = -(A*p0.x + B*p0.y + C*p0.z);
			return *this;
		}
		TGePlane& set(RealT a, RealT b, RealT c, RealT d)
		{
			A = a;
			B = b;
			C = c;
			D = d;
			return *this;
		}
		value_type dist(const PointType&pt) const
		{
			value_type v = A*A + B*B + C*C;
			if (v < 1.0e-6)return 0;
			value_type a = abs(A*pt.x + B*pt.y + C*pt.z + D);
			return a / sqrt(v);
		}
		value_type evaluate(const PointType&pt) const
		{
			return A*pt.x + B*pt.y + C*pt.z + D;
		}
		value_type evalPoint(const PointType&pt) const
		{
			return A*pt.x + B*pt.y + C*pt.z + D;
		}
		TGePlane&normalize(value_type tol)
		{
			TGeVector3 vec(A, B, C);
			value_type len = vec.length();
			if (len > tol)
			{
				A /= len;
				B /= len;
				C /= len;
				D /= len;
			}
			return *this;
		}
		VectorType normal() const
		{
			return VectorType(A, B, C).normal();
		}
		bool pointOn(const PointType &pt, value_type tol) const
		{
			value_type t1 = sqrt(A*A + B*B + C*C);
			if (t1 < 1.0e-8)return FALSE;
			return fabs(pt.x*A + pt.y*B + pt.z*C + D) / t1 < tol;
		}
		bool isEqual(const TGePlane& pln, value_type tol)const
		{
			if (abs(A - pln.A)>tol || abs(B - pln.B) > tol || abs(C - pln.C) > tol || abs(D - pln.D) > tol)return false;
			return true;
		}
		bool intersectWithRay(
			const PointType&p0
			, const VectorType&dir
			, PointType&ip
			, value_type tol = 1.0e-7) const
		{
			value_type t = A*dir.x + B*dir.y + C*dir.z;
			if (fabs(t) < tol)return false;
			t = -(A*p0.x + B*p0.y + C*p0.z + D) / t;
			ip = p0 + dir*t;
			if (fabs(A*p0.x + B*p0.y + C*p0.z + D) < tol) return true;
			VectorType v = ip - p0;
			t = v.dotProduct(dir);
			return t >= 0;
		}
		bool intersectWithLine(
			const PointType&p0
			, const VectorType&dir
			, PointType &ip,
			RealT tol = 1.0e-8) const
		{
			RealT t = A*dir.x + B*dir.y + C*dir.z;
			if (fabs(t) < tol)return false;
			t = -(A*p0.x + B*p0.y + C*p0.z + D) / t;
			ip = p0 + dir*t;
			return true;
		}
		bool intersectWithPlane(
			const TGePlane&plane
			, PointType&line_p0
			, VectorType&line_dir
			, RealT tol)const
		{
			VectorType normal1, normal2;
			normal1 = normal();
			normal2 = plane.normal();
			VectorType d = normal1.crossProduct(normal2);
			if (d.isZero(tol)) return false;
			RealT s1, s2, sa, sb;
			s1 = -D;
			s2 = -plane.D;
			RealT n1n2dot = normal1.dotProduct(normal2);
			RealT n1normsqr = normal1.dotProduct(normal1);
			RealT n2normsqr = normal2.dotProduct(normal2);
			sa = (s2*n1n2dot - s1*n2normsqr) / (n1n2dot*n1n2dot - n1normsqr*n2normsqr);
			sb = (s1*n1n2dot - s2*n2normsqr) / (n1n2dot*n1n2dot - n1normsqr*n2normsqr);
			line_p0 = (sa*normal1 + sb*normal2).asPoint();
			line_dir = d;
			return true;
		}
	};
	template<typename RealT>
	TGePoint3<RealT> TGePoint3<RealT>::project(const PlaneType &pln, const VECTOR3 &vec) const
	{
		typedef TGePoint3<RealT> POINT;
		POINT ip;
		if(pln.intersectWithLine(*this,vec,ip))
		{
			return ip;
		}
		return *this;
	}
	template<typename RealT>
	TGePoint3<RealT> TGePoint3<RealT>::orthoProject(const PlaneType &pln)const
	{
		typedef TGePoint3<RealT> POINT;
		POINT ip;
		if (pln.intersectWithLine(*this, pln.normal(), ip))
		{
			return ip;
		}
		return *this;
	}
	template<typename RealT>
	TGeVector3<RealT> TGeVector3<RealT>::project( const TGeVector3 &planeNormal, const TGeVector3 &projDir ) const
	{
		typedef TGeVector3<RealT> VEC;
		typedef TGePoint3<RealT> POINT;
		typedef TGePlane<RealT,POINT,VEC>PLANE;
		PLANE pln(POINT(0,0,0),planeNormal);
		POINT pt(x,y,z);
		pt=pt.project(pln,projDir);
		return pt.asVector();
	}
	typedef TGePlane<double,GePoint3d,GeVector3d>GePlane;
}